﻿#pragma once
#include <iostream>
#include <winsock2.h>
#include <ws2tcpip.h>
#include <thread>
#include <atomic>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <cmath>
#include <cstdint>

// 转台控制数据结构 (30字节)
#pragma pack(push, 1) 
struct GimbalCommand {
    uint8_t header1 = 0xAA;      // 帧头1 (固定值)
    uint8_t header2 = 0x55;      // 帧头2 (固定值)

    // 内框控制参数
    uint8_t inner_mode;          // 内框模式 (0x00:失能, 0x01:使能, 0x12:速度模式, 0x13:位置速度模式, 0x14:快速响应位置模式)
    float inner_position;        // 内框位置指令 (单位:度)
    float inner_velocity;        // 内框速度指令 (单位:度/秒)

    // 中框控制参数
    uint8_t middle_mode;         // 中框模式
    float middle_position;       // 中框位置指令 (单位:度)
    float middle_velocity;       // 中框速度指令 (单位:度/秒)

    // 外框控制参数 (横滚角控制)
    uint8_t outer_mode;          // 外框模式
    float outer_position;        // 外框位置指令 (横滚角将赋值到这里，单位:度)
    float outer_velocity;        // 外框速度指令 (单位:度/秒)

    uint8_t checksum;            // 校验和 (字节2-28相加取低8位)
};
#pragma pack(pop) 

// A429数据包结构 (根据您的定义)
#pragma pack(push, 1)
struct A429Packet {
    uint32_t header;          // 包头部标识
    uint32_t sequence;        // 序列号
    float roll_angle;         // 横滚角数据 (单位：度)
    uint32_t status;          // 状态字
    uint16_t checksum;        // 校验和
};
#pragma pack(pop)


class UDPServer {
private:
    SOCKET receiveSocket_;     // 接收A429数据的UDP套接字
    SOCKET sendSocket_;        // 发送给转台的UDP套接字
    std::atomic<bool> running_{ false }; // 服务器运行状态标志
    std::thread receiveThread_; // 接收数据线程
    std::thread processThread_; // 处理数据线程

    // 数据队列和同步机制
    std::queue<float> angleQueue_; // 横滚角数据队列
    std::mutex queueMutex_;        // 队列互斥锁
    std::condition_variable queueCV_; // 队列条件变量
    std::mutex coutMutex_;         // 新增: 用于同步控制台输出
    // 目标转台地址信息
    sockaddr_in targetAddr_{};

    // 网络配置参数
    const uint16_t RECEIVE_PORT = 45001;    // A429数据接收端口(暂定45001)
    const uint16_t TARGET_PORT = 25001;     // 转台目标端口
    const char* TARGET_IP = "192.168.1.10"; // 转台IP地址

public:
    // 构造函数和析构函数

    // INVALID_SOCKET是Winsock中表示无效套接字的常量
    UDPServer() : receiveSocket_(INVALID_SOCKET), sendSocket_(INVALID_SOCKET) {}

    ~UDPServer() {
        stop();
    }

    /**
     * @brief 初始化UDP服务器
     * @return 初始化成功返回true，失败返回false
     */
    bool initialize() {
        // 初始化Winsock库
        WSADATA wsaData;
        if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
            std::cerr << "初始化Winsock库失败: " << WSAGetLastError() << std::endl;
            return false;
        }

        // 创建接收套接字 (用于接收A429数据)
        receiveSocket_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
        if (receiveSocket_ == INVALID_SOCKET) {
            std::cerr << "接收A429数据的Socket创建失败: " << WSAGetLastError() << std::endl;
            WSACleanup();
            return false;
        }

        // 绑定接收套接字到指定端口(用于转台接收控制数据)
        sockaddr_in receiveAddr{};
        receiveAddr.sin_family = AF_INET;
        receiveAddr.sin_port = htons(RECEIVE_PORT); // 将一个16位的短整型数（如端口号）从​​主机字节序​​转换为​​网络字节序​​
        receiveAddr.sin_addr.s_addr = htonl(INADDR_ANY); // 监听所有网络接口

        if (bind(receiveSocket_, reinterpret_cast<sockaddr*>(&receiveAddr), sizeof(receiveAddr)) == SOCKET_ERROR) {
            std::cerr << "Bind失败:" << WSAGetLastError() << std::endl;
            closesocket(receiveSocket_);
            WSACleanup();
            return false;
        }

        // 创建发送套接字 (用于向转台发送数据)
        sendSocket_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
        if (sendSocket_ == INVALID_SOCKET) {
            std::cerr << "转台数据发送失败: " << WSAGetLastError() << std::endl;
            closesocket(receiveSocket_);
            WSACleanup();
            return false;
        }

        // 设置目标转台地址
        targetAddr_.sin_family = AF_INET;
        targetAddr_.sin_port = htons(TARGET_PORT);
        inet_pton(AF_INET, TARGET_IP, &targetAddr_.sin_addr);

        // 设置接收套接字为非阻塞模式
        u_long nonBlockingMode = 1;
        if (ioctlsocket(receiveSocket_, FIONBIO, &nonBlockingMode) == SOCKET_ERROR) {
            std::cerr << "设置套接字为非阻塞模式失败: " << WSAGetLastError() << std::endl;
        }

        std::cout << "UDP服务器初始化成功" << std::endl;
        return true;
    }

    /**
     * @brief 启动UDP服务器
     */
    void start() {
        running_ = true;

        // 启动接收线程
        receiveThread_ = std::thread([this]() {
            receiveLoop();
            });

        // 启动处理线程
        processThread_ = std::thread([this]() {
            processLoop();
            });

        std::cout << "三轴转台控制器已启动" << std::endl;
        std::cout << "监听A429数据端口: " << RECEIVE_PORT << std::endl;
        std::cout << "目标转台地址: " << TARGET_IP << ":" << TARGET_PORT << std::endl;
        std::cout << "按Ctrl+C停止程序..." << std::endl;
    }

    /**
     * @brief 停止UDP服务器
     */
    void stop() {
        running_ = false;

        // receiveThread_.joinable()表示正在运行的线程对象

        if (receiveThread_.joinable()) {
            receiveThread_.join();
        }

        if (processThread_.joinable()) {
            processThread_.join();
        }

        if (receiveSocket_ != INVALID_SOCKET) {
            closesocket(receiveSocket_);
        }

        if (sendSocket_ != INVALID_SOCKET) {
            closesocket(sendSocket_);
        }

        WSACleanup();
        std::cout << "服务器已停止" << std::endl;
    }

private:
    /**
     * @brief 接收线程循环函数，负责接收A429数据包
     */
    void receiveLoop() {
        const int BUFFER_SIZE = 1024;
        char buffer[BUFFER_SIZE];

        while (running_) {
            sockaddr_in clientAddr{};
            int clientAddrLen = sizeof(clientAddr);

            // 接收A429数据包
            int receivedBytes = recvfrom(
                receiveSocket_,
                buffer,
                BUFFER_SIZE,
                0,
                // UDP数据报的发送者地址信息将存储在clientAddr中
                reinterpret_cast<sockaddr*>(&clientAddr),
                &clientAddrLen
            );

            if (receivedBytes == SOCKET_ERROR) {
                int error = WSAGetLastError();
                if (error == WSAEWOULDBLOCK) {
                    // 没有数据可用，稍等片刻继续尝试
                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    continue;
                }
                else {
                    std::cerr << "接收A429数据失败: " << error << std::endl;
                    continue;
                }
            }

            // 检查数据包大小至少为A429Packet大小
            if (receivedBytes >= sizeof(A429Packet)) {
                auto packet = reinterpret_cast<A429Packet*>(buffer);

                // 验证校验和 (根据实际A429协议实现)
                if (validateA429Checksum(packet)) {
                    // 提取横滚角并添加到处理队列
                    {
                        std::lock_guard<std::mutex> lock(queueMutex_);
                        angleQueue_.push(packet->roll_angle);
                    }
                    queueCV_.notify_one(); // 通知一个消费者

                    {
						std::lock_guard<std::mutex> lock(coutMutex_); // 同步控制台输出
                        std::cout << "收到横滚角数据: " << packet->roll_angle << "°" << std::endl;
                    }

                }
                else {
                    std::cerr << "A429数据校验失败" << std::endl;
                }
            }
            else {
                std::cerr << "收到无效数据包，大小: " << receivedBytes << "字节" << std::endl;
            }
        }
    }

    /**
     * @brief 处理线程循环函数，负责从队列提取数据并发送给转台
     */
    void processLoop() {
        while (running_) {
            float rollAngle = 0.0f;

            // 等待队列中有数据
            {
                std::unique_lock<std::mutex> lock(queueMutex_);
                // ​​一元谓词指的是任何能够被调用（如函数、Lambda 表达式等）、​​只接受一个参数​​并且​​返回 bool值​​的对象。
                // 它的主要作用是作为一个判断条件，对输入的单个参数进行检验，并根据条件是否满足返回真（true）或假（false）。
                // 这里使用wait_for,第三个参数是一个无参的一元谓词
                // wait_for利用 rel_time设置一个超时计时器。在等待期间，lock会被释放，允许其他线程获取锁并修改共享数据。如果超时发生，wait_for会尝试重新获取 lock，然后返回。
                // 谓词的作用​​：定义线程等待的​​条件​​。线程将在条件为 false时阻塞，直到条件变为 true或因超时返回。​​这是防止虚假唤醒（Spurious Wakeup）的关键​​。
                // 返回值为false: 等待超时​​。即：100毫秒过去了，​​且​​谓词条件仍然为 false（队列依然为空 ​​并且​​ running_仍为 true）。
                // 返回值为true: 谓词条件变为 true（队列不为空 或 running_变为 false）。
                queueCV_.wait_for(lock, std::chrono::milliseconds(100), [this]() {
                    return !angleQueue_.empty() || !running_;
                    });

                if (!running_) break;

                if (!angleQueue_.empty()) {
                    rollAngle = angleQueue_.front();
                    angleQueue_.pop();
                }
                else {
                    continue; // 超时但没有数据，继续等待
                }
            }

            // 创建转台控制命令
            GimbalCommand cmd;

            // 设置控制模式 
            cmd.inner_mode = 0x13;    // 内框位置速度模式
            cmd.middle_mode = 0x13;   // 中框位置速度模式
            cmd.outer_mode = 0x13;    // 外框位置速度模式

            // 设置位置和速度指令(都随横滚角--偏航角)
            cmd.inner_position = rollAngle;     // 内框位置
            cmd.inner_velocity = 5.0f;     // 内框速度

            cmd.middle_position = rollAngle;    // 中框位置
            cmd.middle_velocity = 5.0f;    // 中框速度

            cmd.outer_position = rollAngle; // 将横滚角赋值给外框位置
            cmd.outer_velocity = 5.0f;      // 外框速度 (默认5度/秒)

            // 计算校验和
            cmd.checksum = calculateChecksum(cmd);

            // 发送到转台设备
            int sentBytes = sendto(
                sendSocket_,
                reinterpret_cast<const char*>(&cmd),
                sizeof(cmd),
                0,
                reinterpret_cast<const sockaddr*>(&targetAddr_),
                sizeof(targetAddr_)
            );

            if (sentBytes == SOCKET_ERROR) {
                std::cerr << "发送到转台失败: " << WSAGetLastError() << std::endl;
            }
            else if (sentBytes != sizeof(cmd)) {
                std::cerr << "发送不完整: " << sentBytes << "/" << sizeof(cmd) << "字节" << std::endl;
            }
            else {
				std::lock_guard<std::mutex> lock(coutMutex_); // 同步控制台输出
                std::cout << " 已发送转台控制命令，横滚角: " << rollAngle << "°" << std::endl;
            }

            // 控制发送速率 (根据实际需求调整)
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
    }

    /**
     * @brief 计算转台控制命令的校验和
     * @param cmd 转台控制命令结构体
     * @return 计算得到的校验和
     */
    uint8_t calculateChecksum(const GimbalCommand& cmd) {
        uint8_t sum = 0;
        const uint8_t* data = reinterpret_cast<const uint8_t*>(&cmd);

        // 计算字节2到字节28的和 (跳过header1, header2和checksum字段)
        for (size_t i = 2; i < sizeof(cmd) - 1; ++i) {
            sum += data[i];
        }

        return sum;
    }

    /**
     * @brief 验证A429数据包的校验和
     * @param packet A429数据包指针
     * @return 校验成功返回true，失败返回false
     */
    bool validateA429Checksum(const A429Packet* packet) {
        // 这里需要根据实际的A429协议实现校验和验证
        // 此处为示例实现，需要替换为实际的校验算法
        return true; // 暂时总是返回true
    }
};